/**
 * Author:  Radoslaw Karpowicz
 * Contact: radek.karpowicz@gmail.com
 * Version: 0.1.0
 * Licensed under LGPL v3
 *
 * Based on informations located at:
 * http://wiibrew.org/wiki/Wiimote/Extension_Controllers
 * http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1259091426
 **/
#include <wiring.h>
#undef int
#undef abs
#undef double
#undef float
#undef round

#include <Wire.h>
#include <HardwareSerial.h>
#include <math.h>
#include "duofx.h"

DuoFxController::DuoFXAxis axis;
DuoFxController::Calibration calibr;

bool duofx_initialized = false;
// nunchuk sensors init state
uint8_t init_state[6] = {0x00, 0x01, 0x02, 0xFE, 0x00, 0x05};

void DuoFxController::SetDebugMode(bool debugMode) { debug_mode = debugMode; }
bool DuoFxController::IsEndOfTransmission() { return 0 == Wire.endTransmission() ? true : false; }

/**
 * Execute Controller before Wire.begin(). It is important !!
 */
DuoFxController::DuoFxController() {

  // SCL Frequency = CPU Frequency / (  (16 + (2*TWBR))  * TWSR)
  // TWBR = 400kHz
  TWBR = ((CPU_FREQ / 400000L) - 16) / 2;

  // Set debug mode  
  DuoFxController::SetDebugMode(DEBUG_MODE);
}

DuoFxController::~DuoFxController() { DuoFxController::TurnOffSyncDiode(); }

void DuoFxController::PrintIdent() {

  uint8_t ident[DATA_BYTES_LEN];
  int cnt;

  Wire.beginTransmission(CONTROLLER_ADDR);
  Wire.send(0xFA);

  if ( IsEndOfTransmission() ) {

    Wire.requestFrom(CONTROLLER_ADDR, sizeof(ident));

    for ( cnt = 0; Wire.available() && (cnt < sizeof(ident)); cnt++ ) {

      ident[cnt] = Wire.receive();
    }

    if ( cnt == sizeof(ident) ) {

      Serial.print("Controller ID: ");

      for (int i = 0; i < cnt; i++ ) {

        Serial.print(ident[cnt], HEX);
        Serial.print(" ");
      }

      Serial.println();
    } else {
      
      Serial.print("Ident reading error. Read only ");
      Serial.print(cnt);
      Serial.print(" of ");
      Serial.print(sizeof(ident));
      Serial.print(" bytes.");
      Serial.println();
    }
  } else {

    Serial.println("Can't get Controller ID.");
  }
}

bool DuoFxController::Init() {

  // current timestamp
  unsigned long time = millis();
  duofx_initialized = false;

  if ( debug_mode ) Serial.println("Waiting for Duo FX Controller device."); // debug
  pinMode(LED_PIN, OUTPUT);

  do {
    Wire.beginTransmission(CONTROLLER_ADDR);
    // Init sequence (same as for Nyko Kama controller) - 0x55 [0x00a400f0] 0x00 [0x00a400fb]
    Wire.send(0xF0);
    Wire.send(0x55);

    if ( IsEndOfTransmission() ) {
      
      if ( debug_mode ) Serial.println("Init sequence: 0x55 [0x00a400f0]"); // debug

      Wire.beginTransmission(CONTROLLER_ADDR);
      Wire.send(0xFB);
      Wire.send(0x00);

      if ( IsEndOfTransmission() ) {
      
        if ( debug_mode ) Serial.println("Init sequence: 0x00 [0x00a400fb]"); // debug

        duofx_initialized = true;
      }
    } 

    delay(INIT_TIMEOUT);
  } while ( !duofx_initialized );
  
  // Print Duo FX Controller ID
  if ( debug_mode ) DuoFxController::PrintIdent();

  // Check if nunchuk has been synced correctly
  if ( DuoFxController::IsValidDuoFXController() ) {

    //DuoFxController::CalibrateController();
    DuoFxController::TurnOnSyncDiode();
    return duofx_initialized = true;
  } else {

    DuoFxController::TurnOffSyncDiode();
    if ( debug_mode ) Serial.println("Invalid init state bytes for controller.");
    return duofx_initialized = false;
  }
}

// TODO
void DuoFxController::CalibrateController() {

  if ( debug_mode ) Serial.println("Calibration process has been started.");

  // Save init values to calculate futher angles etc.
  if ( debug_mode ) Serial.println("Press for 2s. Joystick RIGHT"); delay(CALIBRATION_DELAY); DuoFxController::ReceiveCoordinates(1);
  calibr.max_joy_x_axis = axis.joy_x_axis;
  
  if ( debug_mode ) Serial.println("Press for 2s. Joystick LEFT"); delay(CALIBRATION_DELAY); DuoFxController::ReceiveCoordinates(1);
  calibr.min_joy_x_axis = axis.joy_x_axis;
  
  if ( debug_mode ) Serial.println("Press for 2s. Joystick UP"); delay(CALIBRATION_DELAY); DuoFxController::ReceiveCoordinates(1);
  calibr.max_joy_y_axis = axis.joy_y_axis;
  
  if ( debug_mode ) Serial.println("Press for 2s. Joystick DOWN"); delay(CALIBRATION_DELAY); DuoFxController::ReceiveCoordinates(1);
  calibr.min_joy_y_axis = axis.joy_y_axis;
  
  if ( debug_mode ) {
  
    Serial.print("- Joystick X MAX: "); Serial.println(calibr.max_joy_x_axis, DEC);
    Serial.print("- Joystick X MIN: "); Serial.println(calibr.min_joy_x_axis, DEC);
    Serial.print("- Joystick Y MAX: "); Serial.println(calibr.max_joy_y_axis, DEC);
    Serial.print("- Joystick Y MIN: "); Serial.println(calibr.min_joy_y_axis, DEC);
    Serial.println("Calibration has been set up.");
  }
}

bool DuoFxController::IsValidDuoFXController() {
  
  Wire.requestFrom(CONTROLLER_ADDR, sizeof(init_state));

  for (int i = 0; i < sizeof(init_state) && Wire.available(); i++) {

    if ( init_state[i] != Wire.receive() ) return false;
  }

  return true;
}

void DuoFxController::ReceiveCoordinates(int accelRatio = 1) {

  uint8_t buffer[DATA_BYTES_LEN];

  Wire.requestFrom(CONTROLLER_ADDR, DATA_BYTES_LEN);

  for (int i = 0; i < sizeof(buffer) && Wire.available(); i++) {

    buffer[i] = Wire.receive();
  }

  axis.joy_x_axis = buffer[JOY_X_AXIS_ADDR];
  axis.joy_y_axis = buffer[JOY_Y_AXIS_ADDR];

  axis.accel_x_axis = buffer[ACCEL_X_AXIS_ADDR] * accelRatio;
  axis.accel_y_axis = buffer[ACCEL_Y_AXIS_ADDR] * accelRatio;
  axis.accel_z_axis = buffer[ACCEL_Z_AXIS_ADDR] * accelRatio;

  axis.z_button = 0;
  axis.c_button = 0;

  // calculate rest of bytes for z, c button and for accelerometer
  if ((buffer[Z_C_BUTTON_ADDR] >> 0) & 1) axis.z_button = 1;
  if ((buffer[Z_C_BUTTON_ADDR] >> 1) & 1) axis.c_button = 1;
  if ((buffer[Z_C_BUTTON_ADDR] >> 2) & 1) axis.accel_x_axis += 2;
  if ((buffer[Z_C_BUTTON_ADDR] >> 3) & 1) axis.accel_x_axis += 1;
  if ((buffer[Z_C_BUTTON_ADDR] >> 4) & 1) axis.accel_y_axis += 2;
  if ((buffer[Z_C_BUTTON_ADDR] >> 5) & 1) axis.accel_y_axis += 1;
  if ((buffer[Z_C_BUTTON_ADDR] >> 6) & 1) axis.accel_z_axis += 2;
  if ((buffer[Z_C_BUTTON_ADDR] >> 7) & 1) axis.accel_z_axis += 1;

  // Request for next bytes
  DuoFxController::SendNullBytes(NULL_BYTES_LEN);
}

void DuoFxController::SendNullBytes(unsigned int byteCount) {

  for(unsigned int i = 0; i < byteCount; i++)
  {
    Wire.beginTransmission(CONTROLLER_ADDR);
    Wire.send(0x00);
    Wire.endTransmission();
  }
}

void DuoFxController::SerialPrintCoordinates(char* valueSeparator, char* lineSeparator) {

  Serial.print(axis.joy_x_axis, DEC);
  Serial.print(valueSeparator);

  Serial.print(axis.joy_y_axis, DEC);
  Serial.print(valueSeparator);

  Serial.print(axis.accel_x_axis, DEC);
  Serial.print(valueSeparator);

  Serial.print(axis.accel_y_axis, DEC);
  Serial.print(valueSeparator);

  Serial.print(axis.accel_z_axis, DEC);
  Serial.print(valueSeparator);

  Serial.print(axis.z_button, DEC);
  Serial.print(valueSeparator);

  Serial.print(axis.c_button, DEC);
  Serial.print(lineSeparator);
}

int DuoFxController::GetJoystickAngle() { /* TODO */ }

void DuoFxController::TurnOnSyncDiode()  { digitalWrite(LED_PIN, HIGH); }
void DuoFxController::TurnOffSyncDiode() { digitalWrite(LED_PIN, LOW); }
bool DuoFxController::IsInitialized()    { return duofx_initialized; }
